/*
 * Copyright (c) 2010, Jim Hollinger
 * All rights reserved.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions
 * are met:
 *   * Redistributions of source code must retain the above copyright
 *     notice, this list of conditions and the following disclaimer.
 *   * Redistributions in binary form must reproduce the above copyright
 *     notice, this list of conditions and the following disclaimer in the
 *     documentation and/or other materials provided with the distribution.
 *   * Neither the name of Jim Hollinger nor the names of its contributors
 *     may be used to endorse or promote products derived from this
 *     software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR
 * A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT
 * OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL,
 * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT
 * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE,
 * DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY
 * THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
 * OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */

#include "debughelper/debughelper.h"
#include "dtime/dtime.h"
#include "kmlinterface.h"
#include "kmlstatus.h"
#include "kmlopenjauscomponent.h"


using namespace kmljgp;


JausBoolean jausTimeSetTime(JausTime jausTime, time_t timeVal,
                            JausUnsignedShort millisec);

static void kmljgp::processMessage_callback(OjCmpt cmpt, JausMessage rxMessage) {
	if (cmpt != NULL) {

		KmlOpenJausComponent *object = (KmlOpenJausComponent *) ojCmptGetUserData(cmpt);
		if (object != NULL) {
			object->processMessage(cmpt, rxMessage);
		}
	}
}

static void kmljgp::processState_callback(OjCmpt cmpt) {
	if (cmpt != NULL) {

		KmlOpenJausComponent *object = (KmlOpenJausComponent *) ojCmptGetUserData(cmpt);
		if (object != NULL) {
			object->processState(cmpt);
		}
	}
}

KmlOpenJausComponent::KmlOpenJausComponent(KmlInterface *theInterface) :
	jausComponent(NULL),
	kmlInterface(theInterface),
	kmlStatus(NULL)
{
	// create the kml status object
	kmlStatus = new KmlStatus();
}

KmlOpenJausComponent::~KmlOpenJausComponent() {
	stop();

	if (jausComponent != NULL) {
		ojCmptDestroy(jausComponent);
		jausComponent = NULL;
	}
	delete kmlStatus;
	kmlStatus = NULL;
}

bool KmlOpenJausComponent::init(double componentRate_hz) {
	bool flag = false;

	if ((kmlInterface == NULL) || (kmlStatus == NULL)) {
		fprintf(stderr, "KmlOpenJausComponent.init: All objects not created\n");
	} else {

		// create jaus component
		JausByte component_ID = JAUS_GLOBAL_POSE_SENSOR;
		jausComponent = ojCmptCreate("KML Global Pose", component_ID, componentRate_hz);
		if (jausComponent != NULL) {
			ojCmptSetUserData(jausComponent, this);

			// add services of this component
			ojCmptAddService(jausComponent, component_ID);
			ojCmptAddServiceInputMessage (jausComponent, component_ID, JAUS_QUERY_GLOBAL_POSE,  JAUS_BYTE_PRESENCE_VECTOR_ALL_ON);
			ojCmptAddServiceOutputMessage(jausComponent, component_ID, JAUS_REPORT_GLOBAL_POSE, JAUS_BYTE_PRESENCE_VECTOR_ALL_ON);
			ojCmptAddSupportedSc(jausComponent, JAUS_REPORT_GLOBAL_POSE);

			// add component callbacks
			ojCmptSetMessageProcessorCallback(jausComponent, processMessage_callback);
			ojCmptSetStateCallback(jausComponent, JAUS_READY_STATE, processState_callback);

			flag = true;
		} else {
			fprintf(stderr, "KmlOpenJausComponent: Node Manager not running\n");
		}
	}

	return flag;
}

bool KmlOpenJausComponent::start() {
	bool flag = false;

	// set initial state and start the component
	if (jausComponent != NULL) {
		ojCmptSetState(jausComponent, JAUS_READY_STATE);
		if (ojCmptRun(jausComponent) >= 0) {
			printf("KmlOpenJausComponent started\n");
			flag = true;
		} else {
			fprintf(stderr, "KmlOpenJausComponent.start: ojCmptRun failed\n");
		}
	}

	return flag;
}

void KmlOpenJausComponent::stop() {
	if (jausComponent != NULL) {
		ojCmptRemoveSupportedSc(jausComponent, JAUS_REPORT_GLOBAL_POSE);
	}
}

void KmlOpenJausComponent::processMessage(OjCmpt cmpt, JausMessage rxMessage) {
	if ((cmpt != NULL) && (rxMessage != NULL)) {

		// make sure this is a query global pose message
		if (rxMessage->commandCode == JAUS_QUERY_GLOBAL_POSE) {

			// decode query message
			QueryGlobalPoseMessage queryGlobalPose = queryGlobalPoseMessageFromJausMessage(rxMessage);
			if (queryGlobalPose != NULL) {

				// create report message
				ReportGlobalPoseMessage reportGlobalPose = reportGlobalPoseMessageCreate();

				// initialize report message
				jausAddressCopy(reportGlobalPose->destination, queryGlobalPose->source);
				reportGlobalPose->presenceVector = queryGlobalPose->presenceVector;
				reportGlobalPose->sequenceNumber = 0;
				reportGlobalPose->properties.scFlag = 0;

				// fill in report message
				fillInGlobalPose(reportGlobalPose);

				// send report message
				JausMessage txMessage = reportGlobalPoseMessageToJausMessage(reportGlobalPose);
				ojCmptSendMessage(cmpt, txMessage);
				jausMessageDestroy(txMessage);

				reportGlobalPoseMessageDestroy(reportGlobalPose);
				queryGlobalPoseMessageDestroy(queryGlobalPose);
			}
		} else {
			ojCmptDefaultMessageProcessor(cmpt, rxMessage);
		}
	}
}

void KmlOpenJausComponent::processState(OjCmpt cmpt) {
	if (cmpt != NULL) {

		// check for active service connections
		if (ojCmptIsOutgoingScActive(cmpt, JAUS_REPORT_GLOBAL_POSE)) {
			ServiceConnection scList = ojCmptGetScSendList(cmpt, JAUS_REPORT_GLOBAL_POSE);
			if (scList != NULL) {

				// create report message
				ReportGlobalPoseMessage reportGlobalPose = reportGlobalPoseMessageCreate();

				// loop thru pending service connections
				ServiceConnection sc = scList;
				while (sc != NULL) {

					// initialize report message
					jausAddressCopy(reportGlobalPose->destination, sc->address);
					reportGlobalPose->presenceVector = (JausUnsignedShort) sc->presenceVector;
					reportGlobalPose->sequenceNumber = sc->sequenceNumber;
					reportGlobalPose->properties.scFlag = JAUS_SERVICE_CONNECTION_MESSAGE;

					// fill in report message
					fillInGlobalPose(reportGlobalPose);

					// send report message
					JausMessage txMessage = reportGlobalPoseMessageToJausMessage(reportGlobalPose);
					ojCmptSendMessage(cmpt, txMessage);
					jausMessageDestroy(txMessage);

					sc = sc->nextSc;
				}
                reportGlobalPoseMessageDestroy(reportGlobalPose);
			}
			ojCmptDestroySendList(scList);
		}
	}
}

void KmlOpenJausComponent::fillInGlobalPose(ReportGlobalPoseMessage reportGlobalPose) {
	if ((reportGlobalPose != NULL) && (kmlInterface != NULL) && (kmlStatus != NULL)) {

		// get most recent kml status
		kmlInterface->getStaus(kmlStatus);

		// fill in global pose from kml status
		reportGlobalPose->latitudeDegrees       = kmlStatus->latitude_degN;
		reportGlobalPose->longitudeDegrees      = kmlStatus->longitude_degE;
		reportGlobalPose->elevationMeters       = kmlStatus->altitude_m;
		reportGlobalPose->positionRmsMeters     = 0.0;
		reportGlobalPose->rollRadians           = 0.0;
		reportGlobalPose->pitchRadians          = 0.0;
		reportGlobalPose->yawRadians            = 0.0;
		reportGlobalPose->attitudeRmsRadians	= 0.0;

        time_t seconds = static_cast<time_t> (floor(kmlStatus->time_s));
        JausUnsignedShort millisec = static_cast<JausUnsignedShort>
            (floor((kmlStatus->time_s - seconds) * 1000));
        if (!jausTimeSetTime(reportGlobalPose->time, seconds, millisec)) {
            jausUnsignedShortClearBit(&reportGlobalPose->presenceVector,
                JAUS_POSE_PV_TIME_STAMP_BIT);
        }

		if (!kmlStatus->altitudeValid_flag) {
			jausUnsignedShortClearBit(&reportGlobalPose->presenceVector, JAUS_POSE_PV_ELEVATION_BIT);
		}
		jausUnsignedShortClearBit(&reportGlobalPose->presenceVector, JAUS_POSE_PV_POSITION_RMS_BIT);
		jausUnsignedShortClearBit(&reportGlobalPose->presenceVector, JAUS_POSE_PV_ROLL_BIT);
		jausUnsignedShortClearBit(&reportGlobalPose->presenceVector, JAUS_POSE_PV_PITCH_BIT);
		jausUnsignedShortClearBit(&reportGlobalPose->presenceVector, JAUS_POSE_PV_YAW_BIT);
		jausUnsignedShortClearBit(&reportGlobalPose->presenceVector, JAUS_POSE_PV_ATTITUDE_RMS_BIT);
	}
}

JausBoolean jausTimeSetTime(JausTime jausTime, time_t timeVal,
                            JausUnsignedShort millisec) {
    JausBoolean flag = jausTimeSetTime(jausTime, timeVal);

    if (flag) {
        jausTime->millisec = millisec;
        jausTime->timeStamp = jausTimeGetTimeStamp(jausTime);
    }

    return flag;
}
